"""Super controller."""
from controller import Supervisor

# Super = Supervisor()
# timestep = int(Super.getBasicTimeStep())
# LF_Pos = Super.getFromDef('LF_Foot_Pos')
# RF_Pos = Super.getFromDef('RF_Foot_Pos')
# LB_Pos = Super.getFromDef('LB_Foot_Pos')
# RB_Pos = Super.getFromDef('RB_Foot_Pos')


class S(object):

    def __init__(self):
        self.Super = Supervisor()
        self.timestep = int(self.Super.getBasicTimeStep())
        # print(self.timestep)
        self.agent_receiver = list()
        self.agent_emitter = list()
        self.webots_devices_init()

    def webots_devices_init(self):
        for i in range(4):
            _receiver = self.Super.getDevice("agent{0}_receiver".format(i))
            _emitter = self.Super.getDevice("agent{0}_emitter".format(i))
            self.agent_receiver.append(_receiver)
            self.agent_emitter.append(_emitter)
        for i in range(4):
            self.agent_receiver[i].enable(self.timestep)
            self.agent_receiver[i].setChannel(i)
            self.agent_emitter[i].setChannel(i)
    def run(self):
        while self.Super.step(self.timestep) != -1:
            self.Super.setLabel(0, "AAAAAAAAAAAAAAAAAAAAAAA", 0.5, 0.0, 0.1,
                           0xFF0000, 0.1, "Georgia")
            time = self.Super.getTime()
            # print(time)
            if time  >= 0.3:
                self.Super.getFromDef("Beagle1").getField("translation").setSFVec3f([2, 2, 0.33])
                self.Super.simulationReset()
                # agent0_emitter.send(str(Super.getTime()))
            # print("LF:"+str(LF_Pos.getPosition()))
            # print("RF:"+str(RF_Pos.getPosition()))
            # print("RB:"+str(RB_Pos.getPosition()))
            # print("LB:"+str(LB_Pos.getPosition()))
            if self.agent_receiver[0].getQueueLength() > 0:
                received_agent0_data = self.agent_receiver[0].getString()
                received_agent1_data = self.agent_receiver[1].getString()
                received_agent2_data = self.agent_receiver[2].getString()
                received_agent3_data = self.agent_receiver[3].getString()
                print(received_agent0_data)
                print(received_agent1_data)
                print(received_agent2_data)
                print(received_agent3_data)
                self.agent_receiver[0].nextPacket()
                self.agent_receiver[1].nextPacket()
                self.agent_receiver[2].nextPacket()
                self.agent_receiver[3].nextPacket()

a = S()
a.run()



    

       


